/**
  ******************************************************************************
  * Copyright (C) 2020 - ~, SCUT-RobotLab Development Team
  * @file    QuadrpuedKernel.cpp
  * @author  Mentos Seetoo
  * @brief   Quadruped kernel includes following parts: Dynamics, State Machine,
  *          Controllers and Estimators. These are the basical module of a legged
  *          robot. In this file, we will migrate them into a single class to 
  *          make robot alive and add Application Interfaces for high level contol
  *           and users.			
  * @note    THe following third-party libraries are used:
  *          - STL
  *          - Eigen3
  * 
  * @warning 
  *     - At least C++11 is required.			
  *     - 'SysLog' module is used, Make sure that Syslog had initialized before it
  *       is used.											 
  *     - To view more details please visit: \n
  *       
  ******************************************************************************
  */
//TODO Module is consurting!
//TEST Check calculation of dynamic model.
/* Includes ------------------------------------------------------------------*/
#include "QuadrupedKernel.h"

#define Operation_On_Each_Leg   int leg(0); leg < 4 ; leg++
/* Private defines -----------------------------------------------------------*/
using namespace Quadruped;

/* Founctions ----------------------------------------------------------------*/
template <typename T>
QuadrupedKernel<T>::QuadrupedKernel(std::string thread_name, float period, ThreadType type): 
                    CThread(period, thread_name, type),_CoMCommand(period)
{
    /*Create FSM & init*/
    FSM_Passive<T>* p_Passive = new FSM_Passive<T>(&_GaitScheduler, &_StateEstimator,
                              &_CoMCommand, &_Dynamic,period);
    FSM_StandUp<T>* p_StandUp = new FSM_StandUp<T>(&_GaitScheduler, &_StateEstimator, 
                              &_CoMCommand, &_Dynamic, &_BalanceController, period);
    FSM_Locomotion<T>* p_Locomotion = new FSM_Locomotion<T>(&_GaitScheduler, &_StateEstimator,
                              &_CoMCommand, &_Dynamic, &_BalanceController, period);
    FSM_Jump<T>* p_Jump       = new FSM_Jump<T>(&_GaitScheduler, &_StateEstimator,
                              &_CoMCommand, &_Dynamic, &_BalanceController, period);
    FSM_Bounce<T>* p_Bounce   = new FSM_Bounce<T>(&_GaitScheduler, &_StateEstimator,
                              &_CoMCommand, &_Dynamic, &_BalanceController, period);

    FSM_Base<T>::state_list.push_back((FSM_Base<T>*)p_Passive);
    FSM_Base<T>::state_list.push_back((FSM_Base<T>*)p_StandUp);
    FSM_Base<T>::state_list.push_back((FSM_Base<T>*)p_Locomotion);
    FSM_Base<T>::state_list.push_back((FSM_Base<T>*)p_Jump);
    FSM_Base<T>::state_list.push_back((FSM_Base<T>*)p_Bounce);
    FSM_Base<T>::InitFSM((FSM_Base<T>*)p_Passive);
}

template <typename T>
QuadrupedKernel<T>::~QuadrupedKernel()
{
    //Not clean the memory.
}

template <typename T>
void QuadrupedKernel<T>::init()
{
    /* Build robot models */
    if (!_Dynamic.buildModel())
        throw std::invalid_argument("Dynamics parameters are not initialized");
    _Dynamic.buildActuatorModels();

    /* Init State estimator*/

    /* INit Motion Controller*/
    _BalanceController.SetPGains(0.0, 0.0, 0.0,
                                 0.0, 0.0, 0.0);
    _BalanceController.SetDGains(0.00, 0.00, 0.00,
                                 0.00, 0.00, 0.00);

    /* Init Motion Scheduler*/
    _CoMCommand.SetHeightCommand(0.30f);
    _GaitScheduler.initialize();
    //OPTIMAL 输入参数可以改成不需要获得周期的，这里的接口不合理
    _GaitScheduler.SetPhaseInterval(CThread::getPreSetPeriod()/_GaitScheduler.GetPeriod());
}

template <typename T>
void QuadrupedKernel<T>::task()
{
    //_CoMCommand.CalCoMCommand();
    /*Update models*/
    _UpdateModel();
    
    for(Operation_On_Each_Leg)
    {
        //!< Get current foot location in {M} & {G}
        _StateEstimator.p_foot_M[leg] = _Dynamic.BodyState.rBody_M2B *
                                    (_Dynamic.LegState[leg].p + _Dynamic.getHipLocation(leg));

        _StateEstimator.p_foot_G[leg] = _StateEstimator.p + _StateEstimator.p_foot_M[leg];

        SysLog->Record(_INFO_,"Scheduler","Leg POSITION in B-Frame %d: %f, %f, %f",leg,
                _Dynamic.LegState[leg].p(0), _Dynamic.LegState[leg].p(1), _Dynamic.LegState[leg].p(2));
        SysLog->Record(_DEBUG_,"Scheduler","Leg POSITION in M-Frame %d: %f, %f, %f",leg,
                _StateEstimator.p_foot_M[leg](0), _StateEstimator.p_foot_M[leg](1), _StateEstimator.p_foot_M[leg](2));
    }

    /* Gait Scheduler & Controller*/
    FSM_Base<T>::RunFSM();

    /*Compute joint command in {B}*/
    for(Operation_On_Each_Leg)
    {
        _Dynamic.convertToJointAngle(_Dynamic.LegCommand[leg].p, _Dynamic.LegCommand[leg].q, leg);
        _LegCommand[leg] = _Dynamic.LegCommand[leg];

        SysLog->Record(_DEBUG_,"Scheduler","Leg Command in J-Space %d: %f, %f, %f",leg,
                        _LegCommand[leg].q(0), _LegCommand[leg].q(1), _LegCommand[leg].q(2));
    }

/***************************************************************************************************/
    //DEBUG 查看轨迹
    //matplotlibcpp::plotTraj(2,_LegCommand[LF].q(1), _LegCommand[LF].q(2),_LegCommand[LF].q(0));
    //matplotlibcpp::plotTraj(3,_LegCommand[0].q(0), _LegCommand[0].q(1), _LegCommand[0].q(2));

/***************************************************************************************************/
    
    //DEBUG Check thread cycle index 
    static int cnt = 0;
    printf("Kernel thread is finished: %d\n",cnt++);
}

/**
 * @brief //TODO Map:WBC_Ctrl->update_model() using Formula (3)
 * 
 * @tparam T 
 */
template <typename T>
void QuadrupedKernel<T>::_UpdateModel()
{
    /*!
        Updata state and clear claculation flag.
    */
    _Dynamic.calFootPositions();
    _Dynamic.BodyState.rBody_M2B = ori::rpyToRotMat(_Dynamic.BodyState.rpy);
    _Dynamic.BodyState.orientation = ori::rotationMatrixToQuaternion(_Dynamic.BodyState.rBody_M2B);
}

template class QuadrupedKernel<float>;
template class QuadrupedKernel<double>;
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/
